﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using TinyROS.Messages;
using TinyROS.Exceptions;

namespace TinyROS
{
    namespace Messages
    {
        public class Coordinate3D
        {
            public float X { get; }
            public float Y { get; }
            public float Z { get; }
            public Coordinate3D() : this(0.0f, 0.0f, 0.0f)
            {

            }
            public Coordinate3D(float x, float y) : this(x, y, 0.0f)
            {

            }
            public Coordinate3D(float x,float y,float z)
            {
                this.X = x;
                this.Y = y;
                this.Z = z;
            }
            public float L2Norm
            {
                get => MathF.Sqrt(this.X * this.X + this.Y * this.Y + this.Z * this.Z);
            }
        }
        public class CarStatus
        {
            public Coordinate3D Position { get; }
            public Coordinate3D Orientation { get; }
            public Coordinate3D Velocity { get; }
            public Coordinate3D Acceleration { get; }
            public CarStatus() : this(new(), new(), new(), new())
            {

            }
            public CarStatus(Coordinate3D pos,Coordinate3D ori,Coordinate3D v,Coordinate3D a)
            {
                this.Position = pos ?? new();
                this.Orientation = ori ?? new();
                this.Velocity = v ?? new();
                this.Acceleration = a ?? new();
            }
        }
        public class DiffcarCourse 
        {
            public float Linear { get; }
            public float TurningRadius { get; }
            public DiffcarCourse():this(0.0f,9999.9f)
            {

            }
            public DiffcarCourse(float v):this(v,9999.9f)
            {

            }
            public DiffcarCourse(float v,float r)
            {
                this.Linear = v;
                this.TurningRadius = r;
            }
        }
        public class DiffcarVelocity
        {
            public float Left { get; }
            public float Right { get; }
            public DiffcarVelocity() : this(0.0f, 0.0f)
            {

            }
            public DiffcarVelocity(float l,float r)
            {
                this.Left = l;
                this.Right = r;
            }
        }
        public class Atlas
        {
            public byte[,] Global { get; }
            public byte[,] Local { get; }
            public byte[][,] Reserved { get; } = new byte[5][,];
            public Atlas(byte[,] global) : this(global, null)
            {

            }
            public Atlas(byte[,] global, byte[,] local)
            {
                this.Local = local;
                this.Global = global;
            }

        }
        public class Vector3DMessage : StandardMessage
        {
            public new Coordinate3D Value;
            public Vector3DMessage(Coordinate3D value, object sender) : base("Vector3DMessage", sender)
            {
                this.Value = value;
            }
            public Vector3DMessage(string strValue, object sender) : base("Vector3DMessage", sender)
            {
                try
                {
                    var args = strValue.Split(" ", StringSplitOptions.RemoveEmptyEntries);
                    if(args.Length == 2)
                    {
                        var x = float.Parse(args[0]);
                        var y = float.Parse(args[1]);
                        this.Value = new Coordinate3D(x, y);
                        return;
                    }
                    else if(args.Length == 3)
                    {
                        var x = float.Parse(args[0]);
                        var y = float.Parse(args[1]);
                        var z = float.Parse(args[2]);
                        this.Value = new Coordinate3D(x, y, z);
                        return;
                    }
                    else
                    {
                        throw new ArgumentCountException();
                    }
                }
                catch
                {
                    throw new MessageConstructFailedException();
                }
            }
        }
        public class CarStatusMessage : StandardMessage
        {
            public new CarStatus Value;
            public CarStatusMessage(CarStatus value, object sender) : base("CarStatusMessage", sender)
            {
                this.Value = value;
            }
            public CarStatusMessage(string strValue, object sender) : base("CarStatusMessage", sender)
            {
                try
                {
                    var args = strValue.Split(" ", StringSplitOptions.RemoveEmptyEntries);
                    if(args.Length==2)
                    {
                        var pX = float.Parse(args[0]);
                        var pY = float.Parse(args[1]);
                        var pos = new Coordinate3D(pX, pY);
                        var status = new CarStatus(pos, null, null, null);
                        this.Value = status;
                        return;
                    }
                    else if(args.Length == 12)
                    {
                        var (pX, pY, pZ) = (float.Parse(args[0]), float.Parse(args[1]), float.Parse(args[2]));
                        var (oX, oY, oZ) = (float.Parse(args[3]), float.Parse(args[4]), float.Parse(args[5]));
                        var (vX, vY, vZ) = (float.Parse(args[6]), float.Parse(args[7]), float.Parse(args[8]));
                        var (aX, aY, aZ) = (float.Parse(args[9]), float.Parse(args[10]), float.Parse(args[11]));
                        var pos = new Coordinate3D(pX, pY, pZ);
                        var ori = new Coordinate3D(oX, oY, oZ);
                        var vel = new Coordinate3D(vX, vY, vZ);
                        var acc = new Coordinate3D(aX, aY, aZ);
                        var status = new CarStatus(pos, ori, vel, acc);
                        this.Value = status;
                        return;
                    }
                    else
                    {
                        throw new ArgumentCountException();
                    }
                }
                catch
                {
                    throw new MessageConstructFailedException();
                }
            }
        }
        public class DiffcarCourseMessage : StandardMessage
        {
            public new DiffcarCourse Value;
            public DiffcarCourseMessage(DiffcarCourse value, object sender) : base("DiffcarCourseMessage", sender)
            {
                this.Value = value;
            }
            public DiffcarCourseMessage(string strValue, object sender) : base("DiffcarCourseMessage", sender)
            {
                try
                {
                    var args = strValue.Split(" ", StringSplitOptions.RemoveEmptyEntries);
                    if (args.Length == 1)
                    {
                        var linear = float.Parse(args[0]);
                        this.Value = new DiffcarCourse(linear, 9999.9f);
                        return;
                    }
                    else if (args.Length == 2)
                    {
                        var linear = float.Parse(args[0]);
                        var radius = float.Parse(args[1]);
                        this.Value = new DiffcarCourse(linear, radius);
                        return;
                    }
                    else
                    {
                        throw new ArgumentCountException();
                    }
                }
                catch
                {
                    throw new MessageConstructFailedException();
                }
            }
        }
        public class DiffcarVelocityMessage : StandardMessage
        {
            public new DiffcarVelocity Value;
            public DiffcarVelocityMessage(DiffcarVelocity value, object sender) : base("DiffcarVelocityMessage", sender)
            {
                this.Value = value;
            }
            public DiffcarVelocityMessage(string strValue, object sender) : base("DiffcarVelocityMessage", sender)
            {
                try
                {
                    var args = strValue.Split(" ", StringSplitOptions.RemoveEmptyEntries);
                    if (args.Length == 2)
                    {
                        var left = float.Parse(args[0]);
                        var right = float.Parse(args[1]);
                        this.Value = new DiffcarVelocity(left, right);
                        return;
                    }
                    else
                    {
                        throw new ArgumentCountException();
                    }
                }
                catch
                {
                    throw new MessageConstructFailedException();
                }
            }
        }
        public class MapMessage : StandardMessage
        {
            public new Atlas Value;
            public MapMessage(Atlas value, object sender) : base("MapMessage", sender)
            {
                this.Value = value;
            }
        }
        public class ByteStreamMessage : StandardMessage
        {
            public new byte[] Value;
            public ByteStreamMessage(byte[] value, object sender) : base("ByteStreamMessage", sender)
            {
                this.Value = value;
            }
        }
        public class StringMessage : StandardMessage
        {
            public new string Value;
            public StringMessage(string value, object sender) : base("StringMessage", sender)
            {
                this.Value = value;
            }
        }
    }
    public interface IStringConstructable<T>
    {
         
    }
}
